
#include <asm.h>

.global C_SYMBOL(RGB24StretchBlend2RGB24WMMX)

C_SYMBOL(RGB24StretchBlend2RGB24WMMX):

srcy_16= -0x48
interpolation= -0x44
yrIntFloat_16= -0x40
xrIntFloat_16= -0x3C
dstPitch= -0x38
dstHeight= -0x34
dstWidth= -0x30
dstPt= -0x2C
srcPitch = -0x1C
srcHeight = -0x18
srcWidth= -0x14
srcPt= -0x10

    MOV     R12, SP                 // R12: argument
    STMFD   SP!, {R4-R12,LR}
    SUB     SP, SP, #0x4C
Start:
//.long 0xe7fddefe

    LDMIA   R12, {R4-R11}
    STR     R0, [SP,#0x4C+srcPt]
    STR     R1, [SP,#0x4C+dstPt]
    STR     R2, [SP,#0x4C+srcPitch]
    STR     R3, [SP,#0x4C+dstPitch]

    STR     R4, [SP,#0x4C+srcWidth]
    STR     R5, [SP,#0x4C+srcHeight]
    STR     R6, [SP,#0x4C+dstWidth]
    STR     R7, [SP,#0x4C+dstHeight]

    STR     R8, [SP,#0x4C+xrIntFloat_16]
    STR     R9, [SP,#0x4C+yrIntFloat_16]
    STR     R10, [SP,#0x4C+interpolation]

    SUB     R6, R3, R6, LSL #2       //dstPitch - dstWidth*4;

    LDR     LR, =0xFF
    AND     R11, R11, LR
    ORR     R11, R11, R11, LSL #16
    TMCRR   wR13 , R11, R11

    ORR     LR, LR, LR, LSL #16
    TMCRR   wR14, LR, LR
    LDR     LR, =0xFF
    WSUBH   wR11, wR14, wR13

    WZERO   wR7

    MOV     R11, R2
    MOV     R7, R8
    MOV     R9, R1

    LDR     R10, =0x00000008
    TMCRR   wR15 , R10, R10

    MOV R3, #0

    STR R3, [SP,#0x4C+srcy_16]

    LDR R3, [SP,#0x4C+dstHeight]
    CMP R3, #0
    BLS Done

Header:

    LDR R1, [SP,#0x4C+srcy_16]
    MOV R3, R1, LSL#16
    MOV R8, R3, LSR#24

    MOV R2, R1,LSR#16

    LDR R3, [SP,#0x4C+srcPt]
    MLA R4, R2, R11, R3

    LDR R10, [SP,#0x4C+dstWidth]

    MOV R12, #0
    CMP R10, #0

    MOV R0, R7, LSR #14

    BLE Done

    LDR     R1, [SP,#0x4C+interpolation]
    ANDS   R1,  R1, #1

    BEQ LoopX1

LoopX0:

    MOV R1, R12, LSL#16
    MOV R1, R1, LSR#24        // R1 = u8 , R8 = v8

    MOV     R2, R12,LSR#16
    ADD     R2, R4, R2, LSL #2

    WLDRW wR3, [R2]
    WLDRW wR1, [R2, #4]
    ADD R5, R2, R11
    WLDRW wR2, [R5]
    WLDRW wR0, [R5, #4]

    ADD R2, R2, R0, LSL #3
    PLD [R2,R11]
    PLD [R9,#0x20]

    WUNPCKELUB wR2, wR2
    WUNPCKELUB wR0, wR0
    WUNPCKELUB wR3, wR3
    WUNPCKELUB wR1, wR1

    TBCSTH  wR5, R1
    TBCSTH  wR6, R8

    WSUBH  wR0, wR0, wR2
    WMULUL wR0, wR0, wR5

    WSUBH  wR1, wR1, wR3
    WMULUL wR1, wR1, wR5

    WSLLH   wR2, wR2, wR15
    WSLLH   wR3, wR3, wR15

    WADDH   wR0, wR0, wR2
    WADDH   wR1, wR1, wR3

    WSRLH   wR0, wR0, wR15
    WSRLH   wR1, wR1, wR15

    WSUBH   wR0, wR0, wR1
    WMULUL  wR0, wR0, wR6
    WSLLH   wR1, wR1, wR15
    WADDH   wR0, wR0, wR1
    WSRLH   wR0, wR0, wR15

    WMULUL  wR0, wR0, wR13
    WLDRW   wR10, [R9]
    WSRLH   wR0, wR0, wR15
    WUNPCKELUB wR10, wR10
    WMULUL  wR10, wR10, wR11
    ADD R12, R12, R7
    WSRLH   wR10, wR10, wR15
    WADDH   wR0, wR0, wR10

    WPACKHUS wR0, wR0, wR7

    SUBS R10, R10, #1

    WSTRW   wR0, [R9], #4

    BGT LoopX0

NextLine0:

    LDR R1, [SP,#0x4C+srcy_16]
    LDR R2, [SP,#0x4C+yrIntFloat_16]
    ADD R1, R1, R2
    STR R1, [SP,#0x4C+srcy_16]

    MOV R3, R1, LSL#16
    MOV R8, R3, LSR#24

    MOV R2, R1,LSR#16

    LDR R3, [SP,#0x4C+srcPt]
    ADD R9, R9, R6
    MLA R4, R2, R11, R3

    LDR R10, [SP,#0x4C+dstWidth]
    LDR R2, [SP,#0x4C+dstHeight]

    MOV R12, #0

    SUBS R2, R2, #1
    STR R2, [SP,#0x4C+dstHeight]
    BGT  LoopX0
    B   Done

LoopX1:
@.long 0xe7fddefe

    MOV     R2, R12,LSR#16
    ADD     R2, R4, R2, LSL #2

    WLDRW wR3, [R2]

    ADD R2, R2, R0, LSL #3
    PLD [R2,R11]
    PLD [R9,#0x20]

    WUNPCKELUB wR0, wR3
    WMULUL  wR0, wR0, wR13
    WLDRW   wR10, [R9]
    WSRLH   wR0, wR0, wR15
    WUNPCKELUB wR10, wR10
    WMULUL  wR10, wR10, wR11
    ADD R12, R12, R7
    WSRLH   wR10, wR10, wR15
    WADDH   wR0, wR0, wR10

    WPACKHUS wR0, wR0, wR7

    SUBS R10, R10, #1

    WSTRW   wR0, [R9], #4

    BGT LoopX1

NextLine1:

    LDR R1, [SP,#0x4C+srcy_16]
    LDR R2, [SP,#0x4C+yrIntFloat_16]
    ADD R1, R1, R2
    STR R1, [SP,#0x4C+srcy_16]

    MOV R3, R1, LSL#16
    MOV R8, R3, LSR#24

    MOV R2, R1,LSR#16

    LDR R3, [SP,#0x4C+srcPt]
    ADD R9, R9, R6
    MLA R4, R2, R11, R3

    LDR R10, [SP,#0x4C+dstWidth]
    LDR R2, [SP,#0x4C+dstHeight]

    MOV R12, #0

    SUBS R2, R2, #1
    STR R2, [SP,#0x4C+dstHeight]
    BGT  LoopX1

Done:
    ADD     SP, SP, #0x4C
    LDMFD   SP, {R4-R11,SP,PC}

